/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: servos.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <math.h>
#include <pthread.h>
#include <time.h>

#include "serial.h"
#include "globaldefs.h"
#include "utilities.h"
#include "servos.h"


void sendActions(struct servos *servosAction)
{	
	int 	    nbytes=0,i;
	int 	  	sPort;
	short int	servoint=0;
	byte		chk0=0x00, chk1=0x00;
	byte        actPacket[38]={SYNC0,SYNC1,0x14,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

	/*********************************************************************
	 *Scaling Flaps Control Action
	*********************************************************************/	
			
	//servoint = (short int)(((servosAction->roll)*R2D)*21.5+1500);
	servoint = servosAction->yaw;	
	//printf("[servo:]%d \n ",servoint);
	//servoint=1900;		
	if (servoint>1900)
		{servoint=1900;}
	if (servoint<1100)
		{servoint=1100;}
	//servoint=1900;
	//printf("[servo:] %3d \n",servoint);
	actPacket[10] =(unsigned char)(servoint>>8);
	actPacket[11] = (unsigned char)(servoint);	
	/*********************************************************************
	 *Scaling Elevator Control Action
	*********************************************************************/	
	
	servoint = (servosAction->pitch);
	//printf("[Valor accion pitch servo:]%f \n ",(servosAction->pitch));
	//printf("[servo:]%3d \n ",(short int)(((servosAction->pitch)*R2D)*21.5+1500));
	
	if (servoint>1900)
		{servoint=1900;}
	if (servoint<1100)
		{servoint=1100;}
	//servoint=1900;
	//printf("[servo:] %3d \n",servoint);
	actPacket[6] =(unsigned char)(servoint>>8);
	actPacket[7] = (unsigned char)(servoint);
	
	/*********************************************************************
	 *Scaling Rudder Control Action
	*********************************************************************/	
		
	//servoint = (short int)(((servosAction->yaw)*R2D)*21.5+1500);
	servoint = servosAction->roll;	
	//printf("[servo:]%d \n ",servoint);
		
	if (servoint>1900)
		{servoint=1900;}
	if (servoint<1100)
		{servoint=1100;}
	//servoint=1100;
	//printf("[servo:] %3d \n",servoint);
	actPacket[8] =(unsigned char)(servoint>>8);
	actPacket[9] = (unsigned char)(servoint);
	
	/*********************************************************************
	 *Scaling Thrust Control Action
	*********************************************************************/	
		
	servoint = (short int)((servosAction->thrust)*21.5+1500);
		
	//printf("[servo:]%d \n ",servoint);
		
	if (servoint>1900)
		{servoint=1900;}
	if (servoint<1100)
		{servoint=1100;}
	//servoint=1900;
	//printf("[servo:] %3d \n",servoint);
	actPacket[12] =(unsigned char)(servoint>>8);
	actPacket[13] = (unsigned char)(servoint);
	
	
	/*********************************************************************
	 *Opening Serial Port 4
	*********************************************************************/
	sPort = open_serial(SERIAL_PORT1,BAUDRATE_115200);
	/*********************************************************************
	 *Checksum Define
	*********************************************************************/
	for(i=2;i<36;i++)
	{
		chk0 = chk0 + actPacket[i];
		chk1 = chk1 + chk0;
	}
	actPacket[36] = chk0;
	actPacket[37] = chk1;
	/*********************************************************************
	 *Send ADC Command to ADC
	*********************************************************************/
	nbytes = write(sPort,(char*)&actPacket, 38);  
	//nbytes = write(sPort,(char*)&ADCpacketRequest, 7);
	//printf("Write ADC Function %d\n",nbytes);
	/*********************************************************************
	*Close Serial Port 4
	********************************************************************/
	close(sPort);
	/*********************************************************************
	 *Read packet contents
	*********************************************************************/
}

